package com.yDev.rockclimber;

import javax.crypto.spec.PSource;

import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Matrix;
import android.graphics.Paint;
import android.graphics.Point;
import android.text.style.LineHeightSpan.WithDensity;
import android.util.Log;

public class BodyPart {

	public static Context context;

	public static Paint paint;

	// public static Canvas canvas;

	final static double SPEED = 20;

	String bodyPart = "";
	Point endPoint = new Point();

	double deltaX, deltaY;
	double nextX, nextY;

	public Point position1 = new Point();
	public Point position2 = new Point();
	public Point position3 = new Point();

	Boolean animate = false;

	public double size;
	public double width;
	public double height;
	
	public double secondary_width;
	public double secondary_height;
	
	public int energy;

	Bitmap mainBitmap;
	Bitmap secondBitmap;
	Matrix m1;
	Matrix m2;

	public BodyPart() {
		size = 50;
		m1 = new Matrix();
		m2 = new Matrix();
	}

	public BodyPart(int image1, int image2, int scalex, int scaley) {
		mainBitmap = Bitmap.createScaledBitmap(
				BitmapFactory.decodeResource(context.getResources(), image1),
				scalex, scaley, true);
		secondBitmap = Bitmap.createScaledBitmap(
				BitmapFactory.decodeResource(context.getResources(), image2),
				scalex, scaley, true);

		width=scalex;
		height=scaley;
		
		size = 50;
		m1 = new Matrix();
		m2 = new Matrix();
	}
	
	public BodyPart(int image1, int image2, int scalex, int scaley, int scalex2, int scaley2) {
		mainBitmap = Bitmap.createScaledBitmap(
				BitmapFactory.decodeResource(context.getResources(), image1),
				scalex, scaley, true);
		secondBitmap = Bitmap.createScaledBitmap(
				BitmapFactory.decodeResource(context.getResources(), image2),
				scalex2, scaley2, true);

		width=scalex;
		height=scaley;
		
		secondary_width=scalex2;
		secondary_height=scaley2;
		
		size = 50;
		m1 = new Matrix();
		m2 = new Matrix();
	}

	public void animateTo(Point to) {
		endPoint = to;
		double hypo = Math.hypot(position3.x - endPoint.x, position3.y
				- endPoint.y);

		deltaX = (endPoint.x - position3.x) / hypo;
		deltaY = (endPoint.y - position3.y) / hypo;

		deltaX *= SPEED;
		deltaY *= SPEED;

		nextX = position3.x;
		nextY = position3.y;

		animate = true;
	}

	public void animateTorsoTo(Point clickedAt) {

		deltaY = -5;

		nextX = position1.x;
		nextY = position1.y;

		animate = true;
	}

	public void moveTorsoUp(BodyPart Head, BodyPart RightArm, BodyPart LeftArm,
			BodyPart RightLeg, BodyPart LeftLeg) {

		if (animate) {
			Boolean hypoRA = Math.hypot(RightArm.position1.x
					- RightArm.position3.x, RightArm.position1.y
					- RightArm.position3.y) <= RightArm.size * 2
					|| RightArm.position1.y > RightArm.position3.y;
			Boolean hypoLA = Math.hypot(LeftArm.position1.x
					- LeftArm.position3.x, LeftArm.position1.y
					- LeftArm.position3.y) <= LeftArm.size * 2
					|| LeftArm.position1.y > LeftArm.position3.y;
			Boolean hypoRL = Math.hypot(RightLeg.position1.x
					- RightLeg.position3.x, RightLeg.position1.y
					- RightLeg.position3.y) <= RightLeg.size * 2;
			Boolean hypoLL = Math.hypot(LeftLeg.position1.x
					- LeftLeg.position3.x, LeftLeg.position1.y
					- LeftLeg.position3.y) <= LeftLeg.size * 2;

			if (hypoRA && hypoLA && hypoRL && hypoLL) {
				nextX += deltaX;
				nextY += deltaY;

				position1 = new Point((int) nextX, (int) nextY);
				position2 = new Point(position1.x, (int) (position1.y + size));

				updateToTorso(Head, this, RightArm, LeftArm, RightLeg, LeftLeg);
			} else {
				animate = false;
			}
		} else {
			if (!RightArm.animate)
				RightArm.endPoint = new Point(RightArm.position3);

			if (!LeftArm.animate)
				LeftArm.endPoint = new Point(LeftArm.position3);

			if (!RightLeg.animate)
				RightLeg.endPoint = new Point(RightLeg.position3);

			if (!LeftLeg.animate)
				LeftLeg.endPoint = new Point(LeftLeg.position3);
		}
	}

	public void nextPosition() {
		if (animate) {
			if (!stopMoving()) {
				double hypo = Math.hypot(position3.x - endPoint.x, position3.y
						- endPoint.y);
				deltaX = (endPoint.x - position3.x) / hypo;
				deltaY = (endPoint.y - position3.y) / hypo;

				deltaX *= SPEED;
				deltaY *= SPEED;

				nextX += deltaX;
				nextY += deltaY;

				position3 = new Point((int) nextX, (int) nextY);

				if (bodyPart.compareTo("RightLeg") == 0
						|| bodyPart.compareTo("LeftLeg") == 0) {
					updateLegsTo(position3);
				} else if (bodyPart.compareTo("RightArm") == 0)
					updateRightArmTo(position3);
				else if (bodyPart.compareTo("LeftArm") == 0)
					updateLeftArmTo(position3);
			} else
				animate = false;
		}
	}

	public void offsetBodyPart(int offsetY){
		offsetY=5;
		position1.y+=offsetY;
		position2.y+=offsetY;
		position3.y+=offsetY;
		
		endPoint.y+=offsetY;
	}
	
	public Boolean stopMoving() {
		double hypo = Math.hypot(position1.x - nextX, position1.y - nextY);
		return (closeEnough(position3, endPoint, 10));
	}

	public Boolean closeEnough(Point a, Point b, int tolerence) {
		return ((a.x <= b.x && a.x > b.x - tolerence) || (a.x >= b.x && a.x < b.x
				+ tolerence))
				&& ((a.y <= b.y && a.y > b.y - tolerence) || (a.y >= b.y && a.y < b.y
						+ tolerence));
		// return Math.hypot(a.x - b.x, a.y - b.y)<tolerence;
	}

	public static void updateToTorso(BodyPart Head, BodyPart Torso,
			BodyPart RightArm, BodyPart LeftArm, BodyPart RightLeg,
			BodyPart LeftLeg) {
		Head.position1.x = Torso.position1.x;
		Head.position1.y = (int) (Torso.position1.y - Head.size);

		// RightArm.position1.x = Torso.position1.x;
		RightArm.position1.y = Torso.position1.y + 20;

		RightArm.updateRightArmTo(RightArm.endPoint);

		// LeftArm.position1.x = Torso.position1.x;
		LeftArm.position1.y = Torso.position1.y + 20;

		LeftArm.updateRightArmTo(LeftArm.endPoint);

		// RightLeg.position1.x = Torso.position2.x;
		RightLeg.position1.y = Torso.position2.y;

		RightLeg.updateLegsTo(RightLeg.endPoint);

		// LeftLeg.position1.x = Torso.position2.x;
		LeftLeg.position1.y = Torso.position2.y;

		LeftLeg.updateLegsTo(LeftLeg.endPoint);
	}

	public void updateTorso(BodyPart Head) {
		/*mainBitmap = BitmapFactory.decodeResource(context.getResources(),
				R.drawable.torso);*/

		position1.x = (int) (Head.position1.x-1);
		position1.y = (int) (Head.position1.y + Head.height-1);

		position2.x = position1.x;
		position2.y = (int) (position1.y + size);
	}

	public void updateRightArm(BodyPart Torso) {

		position1.x = (int) (Torso.position1.x - Torso.width/2+10);
		position1.y = Torso.position1.y +30;

		position2.x = (int) (position1.x - width * Math.cos(90))+30;
		position2.y = (int) (position1.y + width * Math.sin(90));

		position3.x = (int) (position2.x - secondary_width * Math.cos(90));
		position3.y = (int) (position2.y + secondary_width * Math.sin(90));

		endPoint = new Point(position3);

		m1.setTranslate(position1.x - mainBitmap.getWidth(), position1.y
				- mainBitmap.getHeight() / 2);
		m2.setTranslate(position2.x - secondBitmap.getWidth(), position2.y
				- secondBitmap.getHeight() / 2);
	}

	public void draw(Canvas canvas, Boolean all) {
		if (all) {
			canvas.drawLine(position1.x, position1.y, position2.x, position2.y,
					paint);
			canvas.drawLine(position2.x, position2.y, position3.x, position3.y,
					paint);

			
			canvas.drawBitmap(secondBitmap, m2, paint);
			canvas.drawBitmap(mainBitmap, m1, paint);
		}

		else {
			canvas.drawLine(position1.x, position1.y, position2.x, position2.y,
					paint);

			canvas.drawBitmap(mainBitmap, position1.x-mainBitmap.getWidth()/2, position1.y, paint);
		}
	}

	public void updateRightArmTo(Point to) {
		double dealta = Math.hypot(to.x - position1.x, to.y - position1.y);
		double angle = Math.atan2(position1.y - to.y, position1.x - to.x);

		// mainBitmap.setPixel(position1.x,
		// position1.y-mainBitmap.getHeight()/2, Color.BLACK);

		if (dealta > width+secondary_width) {
			Log.d("CASE", "CASE: " + 1);
			position2.x = (int) (position1.x - width * Math.cos(angle));
			position2.y = (int) (position1.y + width * Math.sin(-angle));
			m1.setTranslate(position1.x - mainBitmap.getWidth(), position1.y
					- mainBitmap.getHeight() / 2);
			m1.postRotate((float) inDegrees(angle), position1.x, position1.y);

			position3.x = (int) (position2.x - secondary_width * Math.cos(angle));
			position3.y = (int) (position2.y + secondary_width * Math.sin(-angle));

			m2.setTranslate(position2.x - secondBitmap.getWidth(), position2.y
					- secondBitmap.getHeight() / 2);
			m2.postRotate((float) inDegrees(angle), position2.x, position2.y);
		} else {
			Log.d("CASE", "CASE: " + 3);
			// position3=to;

			a1 = Math.acos((Math.pow(dealta, 2) -  Math.pow(
					width, 2)-Math.pow(secondary_width, 2)) / (-2 * size * size));
			a2 = Math
					.acos((Math.pow(secondary_width, 2) - Math.pow(dealta, 2) - Math.pow(
							width, 2)) / (-2 * dealta * width));

			/* double */a3 = Math.atan2(position1.y - to.y, position1.x - to.x);

			/* double */a4 = Math.PI / 2 - a2 - a3;

			/* double */a5 = Math.PI / 2 - a4;

			/* double */a6 = Math.PI - a1 - a5;

			if (to.x > position1.x) {

				position2.x = (int) (position1.x - width * Math.sin(a4));
				position2.y = (int) (position1.y - width * Math.cos(a4));

				m1.setTranslate(position1.x - mainBitmap.getWidth(),
						position1.y - mainBitmap.getHeight() / 2);
				m1.postRotate((float) inDegrees(a4), position1.x, position1.y);

				position3.x = (int) (position2.x - secondary_width * Math.cos(a6));
				position3.y = (int) (position2.y + secondary_width * Math.sin(a6));

				m2.setTranslate(position2.x - secondBitmap.getWidth(),
						position2.y - secondBitmap.getHeight() / 2);
				m2.postRotate((float) inDegrees(a6) - 90, position2.x,
						position2.y);

			}

			else {
				if (position1.y < to.y || inDegrees(a1) > 90) {

					a1 = Math.acos((Math.pow(dealta, 2) -  Math.pow(
							width, 2)-Math.pow(secondary_width, 2)) / (-2 * size * size));
					a2 = Math
							.acos((Math.pow(secondary_width, 2) - Math.pow(dealta, 2) - Math.pow(
									width, 2)) / (-2 * dealta * width));

					a3 = Math.atan2(position1.x - to.x, position1.y - to.y);

					a4 = Math.PI - a2 - a3;

					a5 = Math.PI / 2 - a4;

					a6 = Math.PI / 2 - a1 - a5;

					position2.x = (int) (position1.x - width * Math.sin(a4));
					position2.y = (int) (position1.y + width * Math.cos(a4));

					m1.setTranslate(position1.x - mainBitmap.getWidth(),
							position1.y - mainBitmap.getHeight() / 2);
					m1.postRotate((float) inDegrees(a4) - 90, position1.x,
							position1.y);

					position3.x = (int) (position2.x + secondary_width * Math.sin(a6));
					position3.y = (int) (position2.y - secondary_width * Math.cos(a6));

					m2.setTranslate(position2.x - secondBitmap.getWidth(),
							position2.y - secondBitmap.getHeight() / 2);
					m2.postRotate((float) inDegrees(a6) + 90, position2.x,
							position2.y);

				} else {
					a1 = Math.acos((Math.pow(dealta, 2) -  Math.pow(
							width, 2)-Math.pow(secondary_width, 2)) / (-2 * secondary_width * width));
					a2 = Math
							.acos((Math.pow(secondary_width, 2) - Math.pow(dealta, 2) - Math.pow(
									width, 2)) / (-2 * dealta * width));

					a3 = Math.atan2(position1.y - to.y, position1.x - to.x);

					a4 = Math.PI / 2 - a2 - a3;

					a5 = Math.PI / 2 - a4;

					a6 = Math.PI - a1 - a5;

					position2.x = (int) (position1.x - width * Math.sin(a4));
					position2.y = (int) (position1.y - width * Math.cos(a4));

					m1.setTranslate(position1.x - mainBitmap.getWidth(),
							position1.y - mainBitmap.getHeight() / 2);
					m1.postRotate((float) inDegrees(a4) - 90, position1.x,
							position1.y);

					position3.x = (int) (position2.x - secondary_width * Math.cos(a6));
					position3.y = (int) (position2.y + secondary_width * Math.sin(a6));

					m2.setTranslate(position2.x - secondBitmap.getWidth(),
							position2.y - secondBitmap.getHeight() / 2);
					m2.postRotate((float) inDegrees(a6) + 90, position2.x,
							position2.y);

					if (position3.x < position2.x) {
						a1 = Math.acos((Math.pow(dealta, 2) -  Math.pow(
								width, 2)-Math.pow(secondary_width, 2)) / (-2 * size * size));
						a2 = Math
								.acos((Math.pow(secondary_width, 2) - Math.pow(dealta, 2) - Math.pow(
										width, 2)) / (-2 * dealta * width));

						a3 = Math.atan2(position1.x - to.x, position1.y - to.y);

						a4 = Math.PI - a2 - a3;

						a5 = Math.PI / 2 - a4;

						a6 = Math.PI / 2 - a1 - a5;

						position2.x = (int) (position1.x - width * Math.sin(a4));
						position2.y = (int) (position1.y + width * Math.cos(a4));

						m1.setTranslate(position1.x - mainBitmap.getWidth(),
								position1.y - mainBitmap.getHeight() / 2);
						m1.postRotate((float) inDegrees(a4) - 90, position1.x,
								position1.y);

						position3.x = (int) (position2.x + secondary_width * Math.sin(a6));
						position3.y = (int) (position2.y - secondary_width * Math.cos(a6));

						m2.setTranslate(position2.x - secondBitmap.getWidth(),
								position2.y - secondBitmap.getHeight() / 2);
						m2.postRotate((float) inDegrees(a6) + 90, position2.x,
								position2.y);
					}
				}
			}

			Log.d("Point", "A1 inDegrees:  " + inDegrees(a1));
			Log.d("Point", "A2 inDegrees:  " + inDegrees(a2));
			Log.d("Point", "A3 inDegrees:  " + inDegrees(a3));
			Log.d("Point", "A4 inDegrees:  " + inDegrees(a4));
			Log.d("Point", "A5 inDegrees:  " + inDegrees(a5));
			Log.d("Point", "A6 inDegrees:  " + inDegrees(a6));
		}

		Log.d("Point", "dealta: " + dealta);
		Log.d("Point", "To: " + to.toString());
		Log.d("Point", "Position1: " + position1.toString());
		Log.d("Point", "Position2: " + position2.toString());
		Log.d("Point", "Position3: " + position3.toString());

	}

	double a1, a2, a3, a4, a5, a6;

	public void drawParameters(Canvas canvas) {
		Paint p1 = new Paint();
		p1.setColor(Color.RED);
		p1.setTextSize(12);

		Point tl = new Point();
		Point tr = new Point();
		Point bl = new Point();
		Point br = new Point();

		tl.x = Math.min(position1.x, Math.min(position2.x, position3.x));
		tl.y = Math.min(position1.y, Math.min(position2.y, position3.y));

		tr.x = Math.max(position1.x, Math.max(position2.x, position3.x));
		tr.y = Math.min(position1.y, Math.min(position2.y, position3.y));

		br.x = Math.max(position1.x, Math.max(position2.x, position3.x));
		br.y = Math.max(position1.y, Math.max(position2.y, position3.y));

		bl.x = Math.min(position1.x, Math.min(position2.x, position3.x));
		bl.y = Math.max(position1.y, Math.max(position2.y, position3.y));

		if (canvas != null) {
			canvas.drawLine(tl.x, tl.y, tr.x, tr.y, p1);
			canvas.drawLine(tr.x, tr.y, br.x, br.y, p1);
			canvas.drawLine(br.x, br.y, bl.x, bl.y, p1);
			canvas.drawLine(bl.x, bl.y, tl.x, tl.y, p1);

			canvas.drawText(inDegrees(a1) + "", position2.x, position2.y - 20,
					p1);
			canvas.drawText(inDegrees(a5) + "", position2.x + 30, position2.y,
					p1);
			canvas.drawText(inDegrees(a6) + "", position2.x - 30,
					position2.y + 20, p1);

			p1.setColor(Color.GREEN);
			canvas.drawLine(position1.x, position1.y, position3.x, position3.y,
					p1);
			// canvas.drawText(inDegrees(a2) + "", position3.x, position3.y,
			// p1);
			canvas.drawText(inDegrees(a3) + "", position3.x - 30,
					position3.y - 30, p1);
			/*
			 * canvas.drawText(inDegrees(a4) + "", position3.x + 30, position3.y
			 * + 30, p1);
			 */
			// canvas.drawLine(Math.max(position1.x, position3.x),
			// Math.min(position1.y, position3.y), Math.max(position1.x,
			// position3.x),Math.max(position1.y,Math.max(position2.y,
			// position3.y)), p1);
			
			double m1Angle;
			double deltax, deltay;

			deltax=Math.abs(position1.x-position2.x);
			deltay=Math.abs(position1.y-position2.y);
			
			m1Angle=inDegrees(Math.atan2(deltay, deltax));
			
			canvas.drawText(m1Angle+"", 200, 20, p1);
			
			double deltax2=Math.abs(position2.x-position3.x);
			double deltay2=Math.abs(position2.y-position3.y);
			
			
			
			double m2Angle=inDegrees(Math.atan2(deltay2, deltax2));
			
			canvas.drawText(m2Angle+"", 200, 50, p1);
		}
	}

	public void updateLeftArmTo(Point to) {
		double dealta = Math.hypot(to.x - position1.x, to.y - position1.y);
		double angle = Math.atan2(position1.y - to.y, position1.x - to.x);

		// mainBitmap.setPixel(position1.x,
		// position1.y-mainBitmap.getHeight()/2, Color.BLACK);

		if (dealta > size * 2) {
			Log.d("CASE", "CASE: " + 1);
			position2.x = (int) (position1.x - width * Math.cos(angle));
			position2.y = (int) (position1.y + width * Math.sin(-angle));
			m1.setTranslate(position1.x - mainBitmap.getWidth(), position1.y
					- mainBitmap.getHeight() / 2);
			m1.postRotate((float) inDegrees(angle), position1.x, position1.y);

			position3.x = (int) (position2.x - secondary_width * Math.cos(angle));
			position3.y = (int) (position2.y + secondary_width * Math.sin(-angle));

			m2.setTranslate(position2.x - secondBitmap.getWidth(), position2.y
					- secondBitmap.getHeight() / 2);
			m2.postRotate((float) inDegrees(angle), position2.x, position2.y);
		} else {
			a1 = Math.acos((Math.pow(dealta, 2) -  Math.pow(
					width, 2)-Math.pow(secondary_width, 2)) / (-2 * size * size));
			a2 = Math
					.acos((Math.pow(secondary_width, 2) - Math.pow(dealta, 2) - Math.pow(
							width, 2)) / (-2 * dealta * width));

			/* if (position1.y < to.y) { */
			a3 = Math.PI / 2
					- Math.atan2(position1.x - to.x, position1.y - to.y);
			/*
			 * } else { a3 = Math.PI / 2 - Math.atan2(position1.x - to.x,
			 * position1.y - to.y); }
			 */

			a4 = Math.PI / 2 - a2 - a3;

			a5 = Math.PI / 2 - a4;

			a6 = Math.PI - a1 - a5;

			position2.x = (int) (position1.x - size * Math.sin(a4));
			position2.y = (int) (position1.y - size * Math.cos(a4));

			position3.x = (int) (position2.x - size * Math.cos(a6));
			position3.y = (int) (position2.y + size * Math.sin(a6));
			
			double m1Angle;
			double deltax, deltay;

			deltax=Math.abs(position1.x-position2.x);
			deltay=Math.abs(position1.y-position2.y);
			
			m1Angle=inDegrees(Math.atan2(deltay, deltax));
			
			// Q1
			if (position1.x < position2.x && position1.y > position2.y) {
				m1Angle*=-1;
				m1Angle+=180;
			}
			
			//Q2
			else if (position1.x > position2.x && position1.y > position2.y) {
				//m1Angle+=180;
			} 
			
			//Q3
			else if (position1.x > position2.x && position1.y < position2.y) {
				m1Angle*=-1;
			} 
			
			//Q4
			else if (position1.x < position2.x && position1.y < position2.y) {
				m1Angle+=180;
			}

			m1.setTranslate(position1.x - mainBitmap.getWidth(), position1.y
					- mainBitmap.getHeight() / 2);
			m1.postRotate((float) (m1Angle), position1.x,
					position1.y);
			
			double deltax2=Math.abs(position2.x-position3.x);
			double deltay2=Math.abs(position2.y-position3.y);
			
			double m2Angle=inDegrees(Math.atan2(deltay2, deltax2));
			
			// Q1
			if (position2.x < position3.x && position2.y > position3.y) {
				m2Angle+=180;
				m2Angle*=-1;
			}
			
			//Q2
			else if (position2.x > position3.x && position2.y > position3.y) {
				//m2Angle+=180;
			} 
			
			//Q3
			else if (position2.x > position3.x && position2.y < position3.y) {
				m2Angle*=-1;
			} 
			
			//Q4
			else if (position2.x < position3.x && position2.y < position3.y) {
				m2Angle-=180;
			}

			m2.setTranslate(position2.x - secondBitmap.getWidth(), position2.y
					- secondBitmap.getHeight() / 2);
			m2.postRotate((float) (m2Angle), position2.x, position2.y);
		}

	}

	public void updateLegsTo(Point to) {
		double dealta = Math.hypot(to.x - position1.x, to.y - position1.y);
		double angle = Math.atan2(position1.y - to.y, position1.x - to.x);

		if (dealta > size * 2) {
			Log.d("CASE", "CASE: " + 1);
			position2.x = (int) (position1.x - size * Math.cos(angle));
			position2.y = (int) (position1.y + size * Math.sin(-angle));

			position3.x = (int) (position2.x - size * Math.cos(angle));
			position3.y = (int) (position2.y + size * Math.sin(-angle));
		} else {
			Log.d("CASE", "CASE: " + 3);
			// position3=to;

			double a1 = Math.acos((Math.pow(dealta, 2) - 2 * Math.pow(size, 2))
					/ (-2 * size * size));
			double a2 = Math
					.acos((Math.pow(size, 2) - Math.pow(dealta, 2) - Math.pow(
							size, 2)) / (-2 * dealta * size));

			double a3 = Math.atan2(position1.y - to.y, position1.x - to.x);

			double a4 = Math.PI / 2 - a2 - a3;

			double a5 = Math.PI / 2 - a4;

			double a6 = Math.PI - a1 - a5;

			if (to.x < position1.x) {

				position2.x = (int) (position1.x - size * Math.sin(a4));
				position2.y = (int) (position1.y - size * Math.cos(a4));

				position3.x = (int) (position2.x - size * Math.cos(a6));
				position3.y = (int) (position2.y + size * Math.sin(a6));

			}

			else {
				if (position1.y < to.y || inDegrees(a1) > 90) {

					a1 = Math
							.acos((Math.pow(dealta, 2) - 2 * Math.pow(size, 2))
									/ (-2 * size * size));
					a2 = Math
							.acos((Math.pow(size, 2) - Math.pow(dealta, 2) - Math
									.pow(size, 2)) / (-2 * dealta * size));

					a3 = Math.atan2(position1.x - to.x, position1.y - to.y);

					a4 = Math.PI - a2 - a3;

					a5 = Math.PI / 2 - a4;

					a6 = Math.PI / 2 - a1 - a5;

					position2.x = (int) (position1.x - size * Math.sin(a4));
					position2.y = (int) (position1.y + size * Math.cos(a4));

					position3.x = (int) (position2.x + size * Math.sin(a6));
					position3.y = (int) (position2.y - size * Math.cos(a6));

				} else {
					a1 = Math
							.acos((Math.pow(dealta, 2) - 2 * Math.pow(size, 2))
									/ (-2 * size * size));
					a2 = Math
							.acos((Math.pow(size, 2) - Math.pow(dealta, 2) - Math
									.pow(size, 2)) / (-2 * dealta * size));

					a3 = Math.atan2(position1.y - to.y, position1.x - to.x);

					a4 = Math.PI / 2 - a2 - a3;

					a5 = Math.PI / 2 - a4;

					a6 = Math.PI - a1 - a5;

					position2.x = (int) (position1.x - size * Math.sin(a4));
					position2.y = (int) (position1.y - size * Math.cos(a4));

					position3.x = (int) (position2.x - size * Math.cos(a6));
					position3.y = (int) (position2.y + size * Math.sin(a6));

					if (position3.x < position2.x) {
						a1 = Math.acos((Math.pow(dealta, 2) - 2 * Math.pow(
								size, 2)) / (-2 * size * size));
						a2 = Math
								.acos((Math.pow(size, 2) - Math.pow(dealta, 2) - Math
										.pow(size, 2)) / (-2 * dealta * size));

						a3 = Math.atan2(position1.x - to.x, position1.y - to.y);

						a4 = Math.PI - a2 - a3;

						a5 = Math.PI / 2 - a4;

						a6 = Math.PI / 2 - a1 - a5;

						position2.x = (int) (position1.x - size * Math.sin(a4));
						position2.y = (int) (position1.y + size * Math.cos(a4));

						position3.x = (int) (position2.x + size * Math.sin(a6));
						position3.y = (int) (position2.y - size * Math.cos(a6));
					}
				}
			}

			Log.d("Point", "A1 inDegrees:  " + inDegrees(a1));
			Log.d("Point", "A2 inDegrees:  " + inDegrees(a2));
			Log.d("Point", "A3 inDegrees:  " + inDegrees(a3));
			Log.d("Point", "A4 inDegrees:  " + inDegrees(a4));
			Log.d("Point", "A5 inDegrees:  " + inDegrees(a5));
			Log.d("Point", "A6 inDegrees:  " + inDegrees(a6));
		}

		Log.d("Point", "dealta: " + dealta);
		Log.d("Point", "To: " + to.toString());
		Log.d("Point", "Position1: " + position1.toString());
		Log.d("Point", "Position2: " + position2.toString());
		Log.d("Point", "Position3: " + position3.toString());

	}

	public void updateLeftArm(BodyPart Torso) {
		/*
		 * position1.x = Torso.position1.x; position1.y = Torso.position1.y +
		 * 20;
		 * 
		 * position2.x = (int) (position1.x + size * Math.cos(45)); position2.y
		 * = (int) (position1.y + size * Math.sin(45));
		 * 
		 * position3.x = (int) (position2.x + size * Math.cos(45)); position3.y
		 * = (int) (position2.y + size * Math.sin(45));
		 */

		position1.x = Torso.position1.x + 75;
		position1.y = Torso.position1.y + 80;

		position2.x = (int) (position1.x + size * Math.cos(0));
		position2.y = (int) (position1.y + size * Math.sin(0));

		position3.x = (int) (position2.x + size * Math.cos(0));
		position3.y = (int) (position2.y + size * Math.sin(0));

		endPoint = new Point(position3);

		m1.setTranslate(position1.x + mainBitmap.getWidth(), position1.y
				- mainBitmap.getHeight() / 2);
		m2.setTranslate(position2.x + secondBitmap.getWidth(), position2.y
				- secondBitmap.getHeight() / 2);
	}

	public void updateRightLeg(BodyPart Torso) {
		position1.x = Torso.position2.x;
		position1.y = Torso.position2.y;

		position2.x = (int) (position1.x - size * Math.cos(45));
		position2.y = (int) (position1.y + size * Math.sin(45));

		position3.x = (int) (position2.x - size * Math.cos(45));
		position3.y = (int) (position2.y + size * Math.sin(45));
	}

	public void updateLeftLeg(BodyPart Torso) {
		position1.x = Torso.position2.x;
		position1.y = Torso.position2.y;

		position2.x = (int) (position1.x + size * Math.cos(45));
		position2.y = (int) (position1.y + size * Math.sin(45));

		position3.x = (int) (position2.x + size * Math.cos(45));
		position3.y = (int) (position2.y + size * Math.sin(45));
	}

	public double inDegrees(double rads) {
		return rads * 180 / Math.PI;
	}

	public void updateTorsoTo(Point point) {
		position1.y = point.y;

		// position2.x=(int) (point.x+size);
		position2.y = (int) (point.y + size);

	}

}
